from pymata4 import pymata4
from loguru import logger

from Config import Config

commands = [
    ["WriteGpioValue", "ii"],
    ["WriteGpioValueRet", "i"],

    ["ReadGpioValue", "i"],
    ["ReadGpioValueRet", "?"],

    ["Temperature", ""],
    ["TemperatureRet", "f"],
    ["kError", "s"]
]

confInst = Config().inst()
port = confInst.get("arduino", "port")
baud = confInst.get("arduino", "baudrate")
print(port, baud)
# arduino = PyCmdMessenger.ArduinoBoard(port, baud_rate=baud)
# cmd = PyCmdMessenger.CmdMessenger(arduino, commands)

board = pymata4.Pymata4(port, baud)
board.set_pin_mode_servo(9)
board.set_pin_mode_digital_input_pullup(8)


# def cmd_send_wrap(cmd,)

def digital_write(pin, value):
    try:
        pass
        # cmd.send("WriteGpioValue", pin, value)
    except Exception as e:
        logger.debug(e)

    try:
        pass
        # ret = cmd.receive()
        # print(ret)
    except Exception as e:
        logger.debug(e)


def digital_read(pin):
    try:
        return board.digital_read(pin)
    except Exception as e:
        logger.debug(e)


def servo_write(pin, angle):
    # logger
    try:
        logger.debug(angle)
        return board.servo_write(pin, int(angle))
    except Exception as e:
        logger.debug(e)


def get_temperature():
    try:
        pass
        # cmd.send("Temperature")
    except Exception as e:
        logger.debug(e)

    try:
        pass
        # ret = cmd.receive()
        # print(ret)
        # return ret[1][0]
    except Exception as e:
        logger.debug(e)

    return 0
